/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;
import java.util.Date;

/**
 *
 */
public class BallCounter {

    /*has a variable to represent the number of balls being carried by the ball,
     *with two limit switches to monitor when balls are taken in/shot out.
     */
    private static LimitSwitch m_IncrementSwitch = null;
    private static LimitSwitch m_DecrementSwitch = null;
    private static int m_count = 0;
    //create variables to take the elapsed time between ball lifts for Increment limit switch
    private static double start_Time_Inc = 0;
    private static double current_Time_Inc;
    private static double elapsed_Time_Inc = 1.0;
    //create variables to take the elapsed time between ball lifts for Increment limit switch
    private static double start_Time_Dec = 0;
    private static double current_Time_Dec;
    private static double elapsed_Time_Dec = 1.0;

    /*
     * boolean variables for the Increment Switch
     * if currently closed, and if closed during the previous teleopUpdate
     */
    private static boolean last_Inc = false;
    private static boolean current_Inc = false;

    /*
     * boolean variables for the Decrement Switch
     * if currently closed, and if closed during the previous teleopUpdate
     */
    private static boolean last_Dec = false;
    private static boolean current_Dec = false;

    static {
        m_IncrementSwitch = new LimitSwitch(DigitalSidecar.INPUT_INCREMENT_SWITCH);
        m_DecrementSwitch = new LimitSwitch(DigitalSidecar.INPUT_DECREMENT_SWITCH);
    }

    /**
     * This function is called continuously while in teleoperated mode
     */
    public static void teleopUpdate() {

        //sets the most recent state of Increment Switch to "last"
        //and determines if it is currently closed
        last_Inc = current_Inc;
        current_Inc = m_IncrementSwitch.isClosed();

        //sets the most recent state of Decrement Switch to "last"
        //and determines if it is currently closed
        last_Dec = current_Dec;
        current_Dec = m_DecrementSwitch.isClosed();

        //adds 1 to the ball count if Increment Switch is pushed
        //if(currently closed but previously open)
        //takes the start time inside the loop
        //makes sure elapsed time has been long enough
        if (current_Inc && !last_Inc && elapsed_Time_Inc > 0.5) {
            m_count = m_count + 1;
            start_Time_Inc = (new Date()).getTime() / 1000;
        }

        //takes the current time and finds the elapsed time since the ball was counted
        current_Time_Inc = (new Date()).getTime() / 1000;
        elapsed_Time_Inc = current_Time_Inc - start_Time_Inc;


        //subtracts 1 from the ball count if Decrement Switch is pushed
        //if(currently closed but previously open)
        //takes the start time inside the loop
        //makes sure elapsed time has been long enough
        if (current_Dec && !last_Dec && elapsed_Time_Dec > 0.5) {
            m_count = m_count - 1;
            start_Time_Dec = (new Date()).getTime() / 1000;
        }

        //takes the current time and finds the elapsed time since the ball was counted
        current_Time_Dec = (new Date()).getTime() / 1000;
        elapsed_Time_Dec = current_Time_Dec - start_Time_Dec;

        if (ShooterStick.ResetCounterPressed()) {
            m_count = 0;
        }


    }

    public static int getBallCount() {

        return m_count;
    }

    /*
     * Used only by tests.
     */
    public static void teleopUpdate(LimitSwitch incrementSwitch, LimitSwitch decrementSwitch) {
        m_IncrementSwitch = incrementSwitch;
        m_DecrementSwitch = decrementSwitch;

        teleopUpdate();
    }
}
